#include <tasks_manager/task_manager_node.h>


/*******************************************
 * 默认任务组构造
 *******************************************/

// 是P2P任务
bool TaskManagerNode::IsP2PTask(TaskType type){
    if(type == TaskType::P2P_TASK) return true;
    return false;
}

// 是充电任务
bool TaskManagerNode::IsChargingTask(TaskType type){
    if(type == TaskType::CHARGING_TASK) return true;
    return false;
}


// 构造P2P任务 p2p_move
TaskGroup TaskManagerNode::GenP2PTask(geometry_msgs::Pose goal){
    TaskGroup task_group;
    task_group.group_name = "p2p_move";
    task_group.group_id = GenerateId();
    // 构造单个任务
    TaskSingle task_single;
    task_single.task_base.task_name = "p2p_move";
    task_single.task_base.task_id = GenerateId();
    task_single.task_base.map_base.name = __map_name;
    task_single.task_base.group_name = task_group.group_name;
    task_single.task_base.group_id = task_group.group_id;
    task_single.task_base.task_type = TaskType::P2P_TASK;
    task_single.task_base.pose = goal;
    task_group.task_list.emplace_back(task_single);
    return task_group;
}

// 构造梯控转移任务 trans_floor
TaskGroup TaskManagerNode::GenTransTask(v3_msgs::MapBase map_base, v3_msgs::MapBase next_map_base){
    TaskGroup task_group;
    // task_group.group_name = "trans_floor";
    // task_group.group_id = GenerateId();
    // // 构造单个任务
    // TaskSingle task_single;
    // task_single.task_base.task_name = "trans_floor";
    // task_single.task_base.task_id = GenerateId();
    // task_single.task_base.map_base = map_base;
    // task_single.task_base.next_map_base = next_map_base;
    // task_single.task_base.group_name = task_group.group_name;
    // task_single.task_base.group_id = task_group.group_id;
    // task_single.task_base.task_type = TaskType::ELEVATOR_TRANS_TASK;
    // task_group.task_list.emplace_back(task_single);
    return task_group;
}

// 构造回桩任务 charging
TaskGroup TaskManagerNode::GenChargingTask(geometry_msgs::Pose goal){
    TaskGroup task_group;
    task_group.group_name = "charging";
    task_group.group_id = GenerateId();
    // 构造单个任务
    TaskSingle task_single;
    task_single.task_base.task_name = "charging";
    task_single.task_base.task_id = GenerateId();
    task_single.task_base.map_base.name = __map_name;
    task_single.task_base.group_name = task_group.group_name;
    task_single.task_base.group_id = task_group.group_id;
    task_single.task_base.task_type = TaskType::CHARGING_TASK;
    task_single.task_base.pose = goal;
    task_group.task_list.emplace_back(task_single);
    return task_group;
}

// 生成默认充电桩回桩任务 charging   用于自动回桩和手动下发
Status TaskManagerNode::GenDefaultChargingTask(TaskGroup& task_group){
    Status status;
    try{
        // 校验指针数据
        if(!__map_info_ptr){
            throw(string("map_info_ptr is nullptr!"));
        }
        if(__map_info_ptr->base_info.name != __map_name){
            throw(string("map_name is not equal! __map_name = " + __map_name
                + ", map_info.name = " + __map_info_ptr->base_info.name));
        }
        // 查询标记点信息
        bool find_default = false;
        v3_msgs::MarkerPoint default_marker;
        // 查询默认充电桩信息
        for(auto& marker_point : __map_info_ptr->marker_point_list){
            if(marker_point.is_default && marker_point.type == marker_point.CHARGING_PILE){
                find_default = true;
                default_marker = marker_point;
                break;
            }
        }
        if(find_default == false){
            throw(string("can't find default charging point!")); 
        }
        LogPub("INFO", "[GenChargingTask] find " + default_marker.name);
        task_group = GenChargingTask(default_marker.pose);
    }
    catch(string& error){
        status.success = false;
        status.message = error;
        LogPub("ERROR", "[GenChargingTask] error: " + error);
    }
    return status;
}


// 生成充电桩回桩任务(默认充电桩查询失败，前往最近充电桩) charging  用于自动回桩
Status TaskManagerNode::GenNearestChargingTask(TaskGroup& task_group){
    Status status;
    try{
        // 校验指针数据
        if(!__map_info_ptr){
            throw(string("map_info_ptr is nullptr!"));
        }
        if(__map_info_ptr->base_info.name != __map_name){
            throw(string("map_name is not equal! __map_name = " + __map_name
                + ", map_info.name = " + __map_info_ptr->base_info.name));
        }
        // 获取当前机器人位姿
        geometry_msgs::PoseStamped robot_posestamp;
        if(!GetTFPose("map", "base_link", robot_posestamp)){
            throw(string("can't get robot pose!"));
        }
        geometry_msgs::Pose robot_pose = robot_posestamp.pose;
        // 查询标记点信息
        float min_distance = std::numeric_limits<float>::infinity();
        v3_msgs::MarkerPoint min_marker;
        // 查询默认充电桩信息
        for(auto& marker_point : __map_info_ptr->marker_point_list){
            if(marker_point.type == marker_point.CHARGING_PILE){
                float dx = marker_point.pose.position.x - robot_pose.position.x;
                float dy = marker_point.pose.position.y - robot_pose.position.y;
                float distance = hypot(dx, dy);
                if(distance < min_distance){
                    min_distance = distance;
                    min_marker = marker_point;
                }
            }
        }
        if(isinf(min_distance)){
            throw(string("can't find charging point!"));
        }
        LogPub("INFO", "[GenChargingTask] find " + min_marker.name);
        task_group = GenChargingTask(min_marker.pose);
    }
    catch(string& error){
        status.success = false;
        status.message = error;
        LogPub("ERROR", "[GenChargingTask] error: " + error);
    }
    return status;
}

// 生成默认停靠点 用于自动回桩
Status TaskManagerNode::GenDefaultStopTask(TaskGroup& task_group){
    Status status;
    try{
        // 校验指针数据
        if(!__map_info_ptr){
            throw(string("map_info_ptr is nullptr!"));
        }
        if(__map_info_ptr->base_info.name != __map_name){
            throw(string("map_name is not equal! __map_name = " + __map_name
                + ", map_info.name = " + __map_info_ptr->base_info.name));
        }
        // 查询标记点信息
        bool find_default = false;
        v3_msgs::MarkerPoint default_marker;
        // 查询默认充电桩信息
        for(auto& marker_point : __map_info_ptr->marker_point_list){
            if(marker_point.is_default && marker_point.type == marker_point.STOP){
                find_default = true;
                default_marker = marker_point;
                break;
            }
        }
        if(find_default == false){
            throw(string("can't find default stop point!")); 
        }
        LogPub("INFO", "[GenDefaultStopTask] find " + default_marker.name);
        task_group = GenP2PTask(default_marker.pose);
    }
    catch(string& error){
        status.success = false;
        status.message = error;
        LogPub("ERROR", "[GenDefaultStopTask] error: " + error);
    }
    return status;
}

// 生成最近停靠点任务 用于自动回桩
Status TaskManagerNode::GenNearestStopTask(TaskGroup& task_group){
    Status status;
    try{
        // 校验指针数据
        if(!__map_info_ptr){
            throw(string("map_info_ptr is nullptr!"));
        }
        if(__map_info_ptr->base_info.name != __map_name){
            throw(string("map_name is not equal! __map_name = " + __map_name
                + ", map_info.name = " + __map_info_ptr->base_info.name));
        }
        // 获取当前机器人位姿
        geometry_msgs::PoseStamped robot_posestamp;
        if(!GetTFPose("map", "base_link", robot_posestamp)){
           throw(string("can't get robot pose!"));
        }
        geometry_msgs::Pose robot_pose = robot_posestamp.pose;
        // 查询标记点信息
        float min_distance = std::numeric_limits<float>::infinity();
        v3_msgs::MarkerPoint min_marker;
        // 查询默认充电桩信息
        for(auto& marker_point : __map_info_ptr->marker_point_list){
            if(marker_point.type == marker_point.STOP){
                float dx = marker_point.pose.position.x - robot_pose.position.x;
                float dy = marker_point.pose.position.y - robot_pose.position.y;
                float distance = hypot(dx, dy);
                if(distance < min_distance){
                    min_distance = distance;
                    min_marker = marker_point;
                }
            }
        }
        if(isinf(min_distance)){
            throw(string("can't find stop point!"));
        }
        LogPub("INFO", "[GenStopTask] find " + min_marker.name);
        task_group = GenP2PTask(min_marker.pose);
    }
    catch(string& error){
        status.success = false;
        status.message = error;
        LogPub("ERROR", "[GenStopTask] error: " + error);
    }
    return status;    
}

/*******************************************
 * 任务队列管理
 *******************************************/
// 生成任务ID
string TaskManagerNode::GenerateId(){
    // 为了云平台兼容之前版本，此处采用与之前一致生成方式
    // ros::Time now = ros::Time::now();
    // return to_string(now.sec) + "_" + to_string(now.nsec);

    /**
     * ratio: 比例，默认为1
     * return： 1970年1月1日00：00：00到当前的毫秒数
    */
    std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
    uint64_t timepoint = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
    std::this_thread::sleep_for(std::chrono::milliseconds(5));    //延时5ms以避免时序冲突   20230423 ft.wang 修复任务添加ID有时相同问题
    return to_string(timepoint);
}


// 查询当前正在执行单个任务的task_id和所在任务队列首个组合任务的索引
pair<string, int> TaskManagerNode::GetExecTaskSingleInfo(){
    pair<string, int> curr_task_info = {"", 0};
    if(__online_tasks_deque.empty()){
        return curr_task_info;
    }
    auto front_taskgroup = __online_tasks_deque.front();
    for(int i=0; i<front_taskgroup->task_list.size(); ++i){
        auto& task_single = front_taskgroup->task_list[i];
        if(task_single.task_echo.task_status == TaskState::EXECUING){
            curr_task_info = {task_single.task_base.task_id, i};
            break;
        }
    }
    return curr_task_info;
}

// 查询组合任务所在任务队列的索引
int TaskManagerNode::GetTaskGroupIndex(string group_id){
    int index = -1;
    for(int i=0; i<__online_tasks_deque.size(); ++i){
        if(__online_tasks_deque[i]->group_id == group_id){
            index = i;
            break;
        }
    }
    return index;
}

// 删除组合任务 基于group_id
bool TaskManagerNode::DeleteTaskGroup(string group_id){
    // 查询任务组所在的序列号
    int idx = GetTaskGroupIndex(group_id);
    cout << "[DeleteTaskGroup] idx=" << idx << endl;
    if(idx == -1){
        LogPub("ERROR", "[DeleteTaskGroup]Can't find task group! group_id=" + group_id);
        return false;
    }
    else{
        if(idx >=0 && idx < __online_tasks_deque.size()){
            __online_tasks_deque.erase(__online_tasks_deque.begin() + idx);
        }
    }
    return true;
}

// 首个组合任务是否全部完成
bool TaskManagerNode::IsFronTaskGroupFinished(){
    if(__online_tasks_deque.empty()){
        return true;
    }
    auto& front_taskgroup = __online_tasks_deque.front();
    for(auto& task_single : front_taskgroup->task_list){
        if(task_single.task_echo.task_status == TaskState::UNKNOWN){
            return false;
        }
    }
    return true;
}



// 返回当前需要执行任务
std::shared_ptr<TaskSingle> TaskManagerNode::FindOnlineTaskSingleExec(bool& is_first){
    // 检测待执行的任务
    std::shared_ptr<TaskSingle> task_single_ptr = nullptr;
    if(__online_tasks_deque.empty()) return task_single_ptr;
    for(int i=0; i<__online_tasks_deque.front()->task_list.size(); ++i){
        TaskSingle& task_single = __online_tasks_deque.front()->task_list[i];
        cout << "[FindOnlineTaskSingleExec] i: " << i << ", task_status: " << __msg_convert.TaskState2Str(task_single.task_echo.task_status) << endl;
        if(task_single.task_echo.task_status == TaskState::UNKNOWN ||
            task_single.task_echo.task_status == TaskState::TERMINATED ||
            task_single.task_echo.task_status == TaskState::EXECUING){
            // 此处存在BUG，待太平修复 终止任务,返回服务后会多发一帧EXECUING回传状态
            // 待补全 检测地图名称，是否插入梯控转移任务 
            task_single_ptr = std::make_shared<TaskSingle>(task_single);
            is_first = i == 0;
            break;
        }  
    }
    return task_single_ptr;
}


// 获取指定任务索引数据
bool TaskManagerNode::GetOnlineTaskIndex(string task_id, string group_id, int& task_index, int&group_index){
    task_index = -1;
    group_index = -1;
    for(int i=0; i<__online_tasks_deque.size(); ++i){
        if(__online_tasks_deque[i]->group_id == group_id){
            group_index = i;
            for(int j=0; j<__online_tasks_deque[i]->task_list.size(); ++j){
                auto& task_single = __online_tasks_deque[i]->task_list[j];
                if(task_single.task_base.task_id == task_id){
                    task_index = j;
                    break;  
                }
            }
            break; 
        }
    }
    if(task_index != -1 || group_index != -1){
        return false;
    }
    return true;
}